// OBB.cpp
//
//////////////////////////////////////////////////////////////////////

#include "OBB.h"

OBB::OBB(Geometry * g):BVolume(g){
	
}

OBB::OBB(const OBBList & list){
	Vector3D Evals;
	Matrix3D Cov;
	Point3D Pt,S;
	float S00=0,S11=0,S22=0,S01=0,S02=0,S12=0;
	int i,j;
	if (!list.size())
		return; 

	int NumPts=list.size()*8;

	for (j=0; j<list.size(); j++) 
	{
		for(i=0;i<8;i++){
			Pt = list[j]->obbV[i];
			S += Pt;
			S00 += (Pt.x*Pt.x);
			S11 += (Pt.y*Pt.y);
			S22 += (Pt.z*Pt.z);
			S01 += (Pt.x*Pt.y);
			S02 += (Pt.x*Pt.z);
			S12 += (Pt.y*Pt.z);
		}
	}

	Cov(0,0) = S00 - S.x*S.x / NumPts;
	Cov(1,1) = S11 - S.y*S.y / NumPts;
	Cov(2,2) = S22 - S.z*S.z / NumPts;
	Cov(0,1) = S01 - S.x*S.y / NumPts;
	Cov(1,2) = S12 - S.y*S.z / NumPts;
	Cov(0,2) = S02 - S.x*S.z / NumPts;
	Cov(1,0) = Cov(0,1);
	Cov(2,0) = Cov(0,2);
	Cov(2,1) = Cov(1,2);

	Matrix3D Evecs=Cov.eigen(Evals); 

	if ((Evecs.col(0)*Evecs.col(1)).dot(Evecs.col(2)) < 0) 
	{
		Evecs(0,2) = -Evecs(0,2);
		Evecs(1,2) = -Evecs(1,2);
		Evecs(2,2) = -Evecs(2,2);
	}

	pR=Evecs;
	
	Matrix3D pRT = pR.Tr();
	Point3D Min, Max;

	Min = Max = Pt = list[0]->obbV[0] * pRT; // USE FIRST PT AS START MIN AND MAX
	for (j=0; j<list.size(); j++) 
	{
		for(i=0;i<8;i++){
			Pt = list[j]->obbV[i] * pRT;

			if (Pt.x>Max.x)
				Max.x=Pt.x;
			else
				if (Pt.x<Min.x)
					Min.x=Pt.x;

			if (Pt.y>Max.y)
				Max.y=Pt.y;
			else 
				if (Pt.y<Min.y)
					Min.y=Pt.y;

			if (Pt.z>Max.z)
				Max.z=Pt.z;
			else 
				if (Pt.z<Min.z)
					Min.z=Pt.z;
		}
	}

	Point3D Mid = (Min+Max) * 0.5; 
	pT = Mid* pR;                  
	Dim = (Max-Min) * 0.5;         

}

void OBB::FindTightFit()
{
  	Matrix3D pRT = pR.Tr();  
	Point3D Min, Max, Pt;

	Min = Max = g->getVertex(0).pos * pRT;
	for (int j=0; j<g->getVertexCount(); j++)
	{
		Pt =g->getVertex(j).pos * pRT;

		if (Pt.x>Max.x)
			Max.x=Pt.x;
		else
			if (Pt.x<Min.x)
				Min.x=Pt.x;

		if (Pt.y>Max.y)
			Max.y=Pt.y;
		else 
			if (Pt.y<Min.y)
				Min.y=Pt.y;

		if (Pt.z>Max.z)
			Max.z=Pt.z;
		else 
			if (Pt.z<Min.z)
				Min.z=Pt.z;
	}

	Point3D Mid = (Min+Max) * 0.5;  // CALCULATE ORIGIN (AVERAGE OF MAXs AND MINs)
	pT = Mid* pR;                   // TRANSFORM ORIG TO RIGHT PLACE IN WORLD
	Dim = (Max-Min) * 0.5;         // CALCULATE BOX RADII (DIMENSIONS)
}


Matrix3D OBB::GetBestOrientation()
{
	Vector3D Evals;

	if (!g)
		return Matrix3D();  // no tris?  exit!

	Matrix3D Evecs=GetCovariance().eigen(Evals); 

	if ((Evecs.col(0)*Evecs.col(1)).dot(Evecs.col(2)) < 0) 
	{
		Evecs(0,2) = -Evecs(0,2);
		Evecs(1,2) = -Evecs(1,2);
		Evecs(2,2) = -Evecs(2,2);
	}

	return Evecs;
}

Matrix3D OBB::GetCovariance()
{
	Matrix3D Cov;
	Point3D Pt,S;
	float S00=0,S11=0,S22=0,S01=0,S02=0,S12=0;

	int NumPts=g->getVertexCount();

	for (int j=0; j<g->getVertexCount(); j++) 
	{
		Pt = g->getVertex(j).pos;
		S += Pt;
		S00 += (Pt.x*Pt.x);
		S11 += (Pt.y*Pt.y);
		S22 += (Pt.z*Pt.z);
		S01 += (Pt.x*Pt.y);
		S02 += (Pt.x*Pt.z);
		S12 += (Pt.y*Pt.z);
	}

	Cov(0,0) = S00 - S.x*S.x / NumPts;
	Cov(1,1) = S11 - S.y*S.y / NumPts;
	Cov(2,2) = S22 - S.z*S.z / NumPts;
	Cov(0,1) = S01 - S.x*S.y / NumPts;
	Cov(1,2) = S12 - S.y*S.z / NumPts;
	Cov(0,2) = S02 - S.x*S.z / NumPts;
	Cov(1,0) = Cov(0,1);
	Cov(2,0) = Cov(0,2);
	Cov(2,1) = Cov(1,2);

	return Cov;
}


int OBB::OverlapVF(ViewFrustum* VF) const
{
	int i, CompletelyIn = 1;

	for (i=0; i < VF->NumPlanes; i++)
	{
		int Overlap = OverlapPlane(VF->Planes[i]);

		if (Overlap==COMPLETEOUT)
			return(COMPLETEOUT);
		else 
			if (Overlap==PARTIAL)
				CompletelyIn=0;
	}

	if (CompletelyIn)
		return(COMPLETEIN);

	#ifdef FARPLANEON
	if (EdgeIntersect(VF->NearPts[0],VF->FarPts[0]))
		return(PARTIAL); 
	#else
	float InT, OutT;
	if (RayIntersect(VF->NearPts[0],VF->ProjDirs[0],InT,OutT))
		return(PARTIAL); 
	#endif

	for (i=1; i < VF->NumSides; i++)
	{
		if (EdgeIntersect(VF->NearPts[i-1],VF->NearPts[i]))
			return(PARTIAL);
		#ifdef FARPLANEON
		if (EdgeIntersect(VF->NearPts[i],VF->FarPts[i]))
			return(PARTIAL);
		#else
		if (RayIntersect(VF->NearPts[i],VF->ProjDirs[i],InT,OutT)) 
			return(PARTIAL);
		#endif
	}

	if (EdgeIntersect(VF->NearPts[(VF->NumSides)-1],VF->NearPts[0]))
		return(PARTIAL); 

	if (VF->EdgeIntersect(obbV[0],obbV[4]))
		return(PARTIAL);
	if (VF->EdgeIntersect(obbV[1],obbV[5]))
		return(PARTIAL);
	if (VF->EdgeIntersect(obbV[2],obbV[6]))
		return(PARTIAL);
	if (VF->EdgeIntersect(obbV[3],obbV[7]))
		return(PARTIAL);
	if (VF->EdgeIntersect(obbV[0],obbV[3]))
		return(PARTIAL);
	if (VF->EdgeIntersect(obbV[3],obbV[2]))
		return(PARTIAL);
	if (VF->EdgeIntersect(obbV[2],obbV[1]))
		return(PARTIAL);
	if (VF->EdgeIntersect(obbV[1],obbV[0]))
		return(PARTIAL);
	if (VF->EdgeIntersect(obbV[4],obbV[7]))
		return(PARTIAL);
	if (VF->EdgeIntersect(obbV[7],obbV[6]))
		return(PARTIAL);
	if (VF->EdgeIntersect(obbV[6],obbV[5]))
		return(PARTIAL);
	if (VF->EdgeIntersect(obbV[5],obbV[4]))
		return(PARTIAL);

	Plane tP;

	tP.CalcCoeffsGivenNormal(-(Xaxis()),obbV[1]);
	if (tP.InOutTest(VF->NearPts[0]))
		return(COMPLETEOUT); 

	tP.CalcCoeffsGivenNormal(Xaxis(),obbV[4]); 
	if (tP.InOutTest(VF->NearPts[0]))
		return(COMPLETEOUT);

	tP.CalcCoeffsGivenNormal(Yaxis(),obbV[5]);
	if (tP.InOutTest(VF->NearPts[0]))
		return(COMPLETEOUT);

	tP.CalcCoeffsGivenNormal(-(Yaxis()),obbV[3]);
	if (tP.InOutTest(VF->NearPts[0]))
		return(COMPLETEOUT);

	tP.CalcCoeffsGivenNormal(Zaxis(),obbV[1]);
	if (tP.InOutTest(VF->NearPts[0]))
		return(COMPLETEOUT);

	tP.CalcCoeffsGivenNormal(-(Zaxis()),obbV[4]);
	if (tP.InOutTest(VF->NearPts[0]))
		return(COMPLETEOUT);

	return(PARTIAL);
};


int OBB::OverlapPlane(Plane P) const
{
	Vector3D N(P.N.dot(Xaxis()), P.N.dot(Yaxis()), P.N.dot(Zaxis())); 
	int iPos, iNeg;

	if(N.x>0)
		if(N.y>0) {
			if(N.z>0) {
				iPos=0;
				iNeg=6;

			} else {
				iPos=4;
				iNeg=2;

			}
		} else {
			if(N.z>0) {
				iPos=3;
				iNeg=5;

			} else {
				iPos=7;
				iNeg=1;

				}
		} else if(N.y>0) {
			if(N.z>0) {
				iPos=1;
				iNeg=7;

			} else {
				iPos=5;
				iNeg=3;

				}
		} else { 
			if(N.z>0) {
				iPos=2;
				iNeg=4;

			} else {
				iPos=6;
				iNeg=0;

			}
		}

	
	if (P.DistToPt(obbV[iNeg]) > 0)
		return(COMPLETEOUT);
	else 
		if (P.DistToPt(obbV[iPos]) <= 0)
			return(COMPLETEIN);
		else	
			return(PARTIAL);
}

int OBB::EdgeIntersect(Point3D Start, Point3D End) const
{
	float InT, OutT;
	Point3D Dir = End-Start;  // CALC DIRECTION VECTOR OF EDGE

	if (RayIntersect(Start, Dir, InT, OutT))
		if (InT<=1 || OutT<=1)
			return(1);
	
	return(0);
}


int OBB::RayIntersect(Point3D Start, Point3D Dir, float& InT, float& OutT) const
{
	float NewInT, NewOutT;     // STORAGE FOR NEW T VALUES
	float MinD, MaxD;          // SLAB PLANE D VALS (DIST ALONG NORMAL TO PLANE)
	float NdotDir, NdotStart;  // STORAGE FOR REPEATED CALCS NEEDED FOR NewT CALC
	InT=-99999, OutT=99999;    // INIT INTERVAL T-VAL ENDPTS TO -/+ "INFINITY"

	// X-SLAB (PARALLEL PLANES PERPENDICULAR TO X-AXIS) INTERSECTION (Xaxis is Normal)
	NdotDir = Xaxis().dot(Dir);         // CALC DOT PROD OF PLANE NORMAL AND RAY DIR
	NdotStart = Xaxis().dot(Start);     // CALC DOT PROD OF PLANE NORMAL AND RAY START PT
	MinD = Xaxis().dot(obbV[6]);        // CALC D-VAL FOR FIRST PLANE OF SLAB (use LBF)
	MaxD = Xaxis().dot(obbV[0]);        // CALC D-VAL FOR SECOND PLANE OF SLAB (use RTN)

	if (NdotDir == 0)              // CHECK IF RAY IS PARALLEL TO THE SLAB PLANES
	{
		if (MinD < MaxD) { 
			if ((NdotStart < MinD) || (NdotStart > MaxD))
				return(0);
		} else {
			if ((NdotStart < MaxD) || (NdotStart > MinD)) 
				return(0);
			}
	} else {
		NewInT = (MinD - NdotStart) / NdotDir;
		NewOutT = (MaxD - NdotStart) / NdotDir;

		if (NewOutT > NewInT){
			if (NewInT>InT)
				InT=NewInT;

			if (NewOutT<OutT)
				OutT=NewOutT;
		} else {
			if (NewOutT > InT)
				InT=NewOutT;

			if (NewInT < OutT)
				OutT=NewInT;
			}

		if (InT > OutT)
			return(0);
	}

	NdotDir = Yaxis().dot(Dir);         // CALC DOT PROD OF PLANE NORMAL AND RAY DIR
	NdotStart = Yaxis().dot(Start);     // CALC DOT PROD OF PLANE NORMAL AND RAY START PT
	MinD = Yaxis().dot(obbV[6]);        // CALC D-VAL FOR FIRST PLANE OF SLAB (use LBF)
	MaxD = Yaxis().dot(obbV[0]);        // CALC D-VAL FOR SECOND PLANE OF SLAB (use RTN)

	if (NdotDir == 0)              // CHECK IF RAY IS PARALLEL TO THE SLAB PLANES
	{
		if (MinD < MaxD) {
			if ((NdotStart < MinD) || (NdotStart > MaxD))
				return(0);
		} else { 
			if ((NdotStart < MaxD) || (NdotStart > MinD)) 
				return(0);
		}
	} else {
		NewInT = (MinD - NdotStart) / NdotDir;
		NewOutT = (MaxD - NdotStart) / NdotDir;
		if (NewOutT > NewInT) {
			if (NewInT>InT)
				InT=NewInT;
			
			if (NewOutT<OutT)
				OutT=NewOutT;
		} else { 
			if (NewOutT > InT)
				InT=NewOutT;

			if (NewInT < OutT)
				OutT=NewInT;
		}

		if (InT > OutT)
			return(0);
	}

	// Z-SLAB (PARALLEL PLANES PERPENDICULAR TO Z-AXIS) INTERSECTION (Zaxis is Normal)
	NdotDir = Zaxis().dot(Dir);         // CALC DOT PROD OF PLANE NORMAL AND RAY DIR
	NdotStart = Zaxis().dot(Start);     // CALC DOT PROD OF PLANE NORMAL AND RAY START PT
	MinD = Zaxis().dot(obbV[6]);        // CALC D-VAL FOR FIRST PLANE OF SLAB (use LBF)
	MaxD = Zaxis().dot(obbV[0]);        // CALC D-VAL FOR SECOND PLANE OF SLAB (use RTN)

	if (NdotDir == 0)              // CHECK IF RAY IS PARALLEL TO THE SLAB PLANES
	{
		if (MinD < MaxD) {
			if ((NdotStart < MinD) || (NdotStart > MaxD))
				return(0);
		} else { 
			if ((NdotStart < MaxD) || (NdotStart > MinD))
				return(0);
		}
	} else {
		NewInT = (MinD - NdotStart) / NdotDir;
		NewOutT = (MaxD - NdotStart) / NdotDir;

		if (NewOutT > NewInT){
			if (NewInT>InT)
				InT=NewInT;

			if (NewOutT<OutT)
				OutT=NewOutT;
		} else {
			if (NewOutT > InT)
				InT=NewOutT;
			
			if (NewInT < OutT)
				OutT=NewInT;
		}
		if (InT > OutT)
			return(0);
	}
	
	if (InT>=0 || OutT>=0)
		return(1);
	else
		return(0);

}


LIB3D_RETURN OBB::updateTransform(const Matrix3D &t)
{
	int i;
	Vector3D sX,sY,sZ,Rpt;  
	sX = Xaxis() * Dim.x; 
	sY = Yaxis() * Dim.y;
	sZ = Zaxis() * Dim.z;

	Rpt = sX+sY+pT;  obbV[0]=Rpt+sZ; obbV[4]=Rpt-sZ; // Right-Top-Near, Right-Top-Far
	Rpt = sY-sX+pT;  obbV[1]=Rpt+sZ; obbV[5]=Rpt-sZ; // Left-Top-Near, Left-Top-Far
	Rpt = pT-sX-sY;  obbV[2]=Rpt+sZ; obbV[6]=Rpt-sZ; // Left-Bottom-Near, Left-Bottom-Far
	Rpt = sX-sY+pT;  obbV[3]=Rpt+sZ; obbV[7]=Rpt-sZ; // Right-Bottom-Near, Right-Bottom-Far

	for(i=0;i<8;i++){
		obbV[i]=obbV[i]*t;
	}
	return LIB3D_OK;
}

LIB3D_RETURN OBB::updateVolume(){
	pR = GetBestOrientation();
	FindTightFit();
	return LIB3D_OK;
}

Vector3D OBB::Xaxis() const { 
	return (pR.col(0)); 
}

Vector3D OBB::Yaxis() const {
	return (pR.col(1)); 
}

Vector3D OBB::Zaxis() const {
	return (pR.col(2)); 
}
